from mpython import *
import time

class DiLu(object):

    def __init__(self):

        self.LEFT = 0
        self.RIGHT = 1
        self.ALL = 2
        self.CW = 0
        self.CCW = 1

        self.S1 = 0
        self.S2 = 1

        self.OFF = 0
        self.ON = 1

        self.MIDDLE = 2
        self.BLACK = 0
        self.WHITE =1

        self.FORWARD = 0
        self.BACKWARD = 1
        self.TURNLEFT = 2
        self.TURNRIGHT = 3

        self.BBB = 0
        self.BBW = 1
        self.BWB = 2
        self.BWW = 3
        self.WBB = 4
        self.WBW = 5
        self.WWB = 6
        self.WWW = 7
        


    def motorRun(self,index,dirction,speed):
        speed = abs(speed)
        if(speed > 255):
            speed = 255
        buf = [0,dirction,speed]
        if((index > 2) or (index < 0)):
            return
        if(index == 0):
            buf[0] = 0x00
            send = bytes(buf)
            i2c.writeto(0x10,send)
        elif(index == 1):
            buf[0] = 0x02
            send = bytes(buf)
            i2c.writeto(0x10,send)
        elif(index == 2):
            buf[0] = 0x00
            send = bytes(buf)
            i2c.writeto(0x10,send)
            buf[0] = 0x02
            send = bytes(buf)
            i2c.writeto(0x10,send)

    def diluRun(self,index,speed):
        speed = abs(speed)
        if(index == 0):
            self.motorRun(2,0,speed)
        elif(index == 1):
            self.motorRun(2,1,speed)
        elif(index == 2):
            self.motorRun(0,0,0)
            self.motorRun(1,0,speed)
        elif(index == 3):
            self.motorRun(1,0,0)
            self.motorRun(0,0,speed)

    def motorStop(self,index):
        buf = [0,0,0]
        if(index == 0):
            buf[0] = 0x00
            send = bytes(buf)
            i2c.writeto(0x10,send)
        elif(index == 1):
            buf[0] = 0x02
            send = bytes(buf)
            i2c.writeto(0x10,send)
        elif(index == 2):
            buf[0] = 0x00
            send = bytes(buf)
            i2c.writeto(0x10,send)
            buf[0] = 0x02
            send = bytes(buf)
            i2c.writeto(0x10,send)


    def servoAngle(self,index,ang):
        buf = [0,ang]

        if ang > 180:
            ang=180
        elif ang < 0:
            ang=0

        if(index == 0):
            buf[0] = 0x14
            send = bytes(buf)
            i2c.writeto(0x10,send)
        elif(index == 1):
            buf[0] = 0x15
            send = bytes(buf)
            i2c.writeto(0x10,send)


    def toggleLED(self,light,toggle):
        leftLedPin = Pin(Pin.P8, Pin.OUT)
        rightLedPin = Pin(Pin.P9, Pin.OUT)

        if light == 0:
            if (toggle == 0):
                leftLedPin.value(0)
            elif (toggle == 1):
                leftLedPin.value(1)
        elif light == 1:
            if (toggle == 0):
                rightLedPin.value(0)
            elif (toggle == 1):
                rightLedPin.value(1)
        elif light == 2:
            if (toggle == 0):
                leftLedPin.value(0)
                rightLedPin.value(0)
            elif (toggle == 1):
                leftLedPin.value(1)
                rightLedPin.value(1)


    def isSatus(self,side,status):
        leftIrsPin = Pin(Pin.P13, Pin.IN)
        middleIrsPin = Pin(Pin.P0, Pin.IN)
        rightIrsPin = Pin(Pin.P14, Pin.IN)

        leftIrs = leftIrsPin.value()
        middleIrs = middleIrsPin.value()
        rightIrs = rightIrsPin.value()

        if side == 0:
            if leftIrs == status:
                return True
            else:
                return False
        
        elif side == 2:
            if middleIrs == status:
                return True
            else:
                return False
        elif side == 1:
            if rightIrs == status:
                return True
            else:
                return False

        
    def getLfValue(self):
        lfvalue = 9
        leftIrsPin = Pin(Pin.P13, Pin.IN)
        middleIrsPin = Pin(Pin.P0, Pin.IN)
        rightIrsPin = Pin(Pin.P14, Pin.IN)

        leftIrs = leftIrsPin.value()
        middleIrs = middleIrsPin.value()
        rightIrs = rightIrsPin.value()

        if leftIrs == 0 and middleIrs == 0 and rightIrs == 0:
            lfvalue = 0
        elif leftIrs == 0 and middleIrs == 0 and rightIrs == 1:
            lfvalue = 1
        elif leftIrs == 0 and middleIrs == 1 and rightIrs == 0:
            lfvalue = 2
        elif leftIrs == 0 and middleIrs == 1 and rightIrs == 1:
            lfvalue = 3
        elif leftIrs == 1 and middleIrs == 0 and rightIrs == 0:
            lfvalue = 4
        elif leftIrs == 1 and middleIrs == 0 and rightIrs == 1:
            lfvalue = 5
        elif leftIrs == 1 and middleIrs == 1 and rightIrs == 0:
            lfvalue = 6
        elif leftIrs == 1 and middleIrs == 1 and rightIrs == 1:
            lfvalue = 7
        else:
           lfvalue = 9

        return lfvalue
    
    def isLfStatus(self,lfstatus):
        '''
        leftIrsPin = Pin(Pin.P13, Pin.IN)
        middleIrsPin = Pin(Pin.P0, Pin.IN)
        rightIrsPin = Pin(Pin.P14, Pin.IN)

        leftIrs = leftIrsPin.value()
        middleIrs = middleIrsPin.value()
        rightIrs = rightIrsPin.value()


    
        if leftIrs == 0 and middleIrs == 0 and rightIrs == 0:
            lfvalue = 0
        elif leftIrs == 0 and middleIrs == 0 and rightIrs == 1:
            lfvalue = 1
        elif leftIrs == 0 and middleIrs == 1 and rightIrs == 0:
            lfvalue = 2
        elif leftIrs == 0 and middleIrs == 1 and rightIrs == 1:
            lfvalue = 3
        elif leftIrs == 1 and middleIrs == 0 and rightIrs == 0:
            lfvalue = 4
        elif leftIrs == 1 and middleIrs == 0 and rightIrs == 1:
            lfvalue = 5
        elif leftIrs == 1 and middleIrs == 1 and rightIrs == 0:
            lfvalue = 6
        elif leftIrs == 1 and middleIrs == 1 and rightIrs == 1:
            lfvalue = 7
        else:
           lfvalue = 9
        '''
        if self.getLfValue() == lfstatus:
            return True
        else:
            return False

    





        





